function dp
% Input, Body 1-2-3 Euler Angles
q(:,1) = [1 2.000000001 3]';

for i = 1:1
    c1(i) = cos(q(1,i));
    c2(i) = cos(q(2,i));
    c3(i) = cos(q(3,i));

    s1(i) = sin(q(1,i));
    s2(i) = sin(q(2,i));
    s3(i) = sin(q(3,i));
end
% -------------------------------------------------------------
% Transformation matrices
for i = 1:1
    pr_P_K(1:3,1:3,i) = [c2(i)*c3(i)                   -c2(i)*s3(i)                    s2(i);
                        s1(i)*s2(i)*c3(i)+s3(i)*c1(i) -s1(i)*s2(i)*s3(i)+c1(i)*c3(i) -s1(i)*c2(i);
                        -c1(i)*s2(i)*c3(i)+s3(i)*s1(i) c1(i)*s2(i)*s3(i)+c3(i)*s1(i)  c1(i)*c2(i);];
end

N_C_K(1:3,1:3,1) = pr_P_K(1:3,1:3,1);

Q0 = 0.5*sqrt(1+pr_P_K(1,1,i)+pr_P_K(2,2,i)+pr_P_K(3,3,i));
Q1 = (pr_P_K(3,2,i)-pr_P_K(2,3,i))/(4*Q0);
Q2 = (pr_P_K(1,3,i)-pr_P_K(3,1,i))/(4*Q0);
Q3 = (pr_P_K(2,1,i)-pr_P_K(1,2,i))/(4*Q0);
Q(1:4,i) = [Q0 Q1 Q2 Q3];

while(1)
    delq = del_quat(Q);
    Q = Q + delq;
    Q = Q/norm(Q);
    pause
end

keyboard;

function op = del_quat(q)

Q0 = q(1);
Q1 =q(2);
Q2 = q(3);
Q3 = q(4);
Q(1:4,1) = [Q0 Q1 Q2 Q3];
E = [-Q1 Q0 -Q3 Q2;
     -Q2 Q3 Q0 -Q1;
     -Q3 -Q2 Q1 Q0];
 
 G = [-Q1 Q0 Q3 -Q2;
     -Q2 -Q3 Q0 Q1;
     -Q3 Q2 -Q1 Q0];
R = E*G';

rp = [1 0 0]';
rp_n = R*rp;

n1 = [1 0 0]';
n2 = [0 1 0]';
n3 = [0 0 1]';

Cq(:,1) = cross(n1,rp_n);
Cq(:,2) = cross(n2,rp_n);
Cq(:,3) = cross(n3,rp_n);

P1 = [Cq;eye(3)];
T1 = [Cq;zeros(3)];
T2 = [zeros(3);eye(3)];
P1(5,2) = 1;
P1(6,3) = 1;
U = orth(Cq);
P2 = [U*U'*C(q);zeros(3,1)];
P3 = [C(q);zeros(3,1)];
norm(C(q));
delq = P1\P3;
tmp = [1 2 3]'
e = [E;Q'];
op = 0.5*e'*[delq;0];
keyboard

function op = C(q)
Q0 = q(1);
Q1 =q(2);
Q2 = q(3);
Q3 = q(4);

E = [-Q1 Q0 -Q3 Q2;
     -Q2 Q3 Q0 -Q1;
     -Q3 -Q2 Q1 Q0];
 
 G = [-Q1 Q0 Q3 -Q2;
     -Q2 -Q3 Q0 Q1;
     -Q3 Q2 -Q1 Q0];
R = E*G';
% Assume that the point P is [1 0 0]' in body basis. If the relative
% Euler angles between body and newtonian frame are [1 2 3], then the
% coordinates of point P expressed in newtonian frame are 
% [ 0.411982245665683  -0.681242720256403   0.605127247241369]'
% Thus, op gives the error between the current joint angles 
op = [ 0.411982245665683  -0.681242720256403   0.605127247241369]' - R*[1 0 0]';
